# ROVER GPS FOLLOW
# JUNE 8, 2012 
# AUTHOR: MANU CHANDRAN, mannsurya@gmail.com
# 

#******************************************************************************************
# version 1.0
#******************************************************************************************

#******************************************************************************************
# TODO
# TIME PLOT GPSX, GPSY, COMMANDS
# Implement heading in simulate bot.
#******************************************************************************************

#******************************************************************************************
# STATE ALGORITHM
#******************************************************************************************
# STATUS = LISTEN;
# STATUS = CONFIG; CONFIGURE ROBOT ; 
# STATUS = INIT; GET INITIAL POSITION AND HEADING; POS = INIT
# STATUS = AUTO ; WHILE ( TIME < MAXTIME ) & (INPUT != 'STOP') & (POS != REACHED); \
	# POS = TRANSIT
	# GET CURRENTPOS
	# GET CURRENTHEAD
	# CALCULATE NEW PATH USING ROBOT CONFIG UNITS
	# GET NEWHEAD BASED ON NEW PATH
	# NEXT 
# STATUS = LISTEN; 

#******************************************************************************************

import msvcrt
import datetime
import serial
import time
import numpy as np
import matplotlib.pyplot as plt
import math

Forward  = chr(0x32)
Left 	 = chr(0x31)
Right	 = chr(0x33)
Stop	 = chr(0x34)
RotL 	 = chr(0x35)

xlist 	 = []
ylist 	 = []
tlist    = []
#******************************************************************************************
#FUNCTIONS
#******************************************************************************************
def fReturnHeading(xstart,ystart,xend,yend): # returns angle in -180deg to 180 deg
	xdiff 	 = float(xend) - float(xstart)
	ydiff 	 = float(yend) - float(ystart)
	OutAngle = 0
	if xdiff == 0:
		xdiff = 0.001
	if ydiff == 0:
		ydiff = 0.000
	if (xdiff > 0) & (ydiff > 0): # 1st Quad
		OutAngle = math.atan(ydiff/xdiff)
	if (xdiff < 0) & (ydiff > 0): # 2nd Quad
		OutAngle = math.atan(ydiff/xdiff) + math.pi
	if (xdiff < 0) & (ydiff < 0): # 3rd Quad
		OutAngle = math.atan(ydiff/xdiff) + math.pi
	if (xdiff > 0) & (ydiff < 0): # 4th Quad
		OutAngle = math.atan(ydiff/xdiff) + 2*math.pi
	OutAngle = OutAngle*360/(2*math.pi) 		
	return OutAngle
		
def fInitGpsPort(): #Returns handle
 	ser = serial.Serial('COM17', 9600, timeout=0.1)
	return ser
def fInitBotPort(): #Returns handle
	ser = serial.Serial('COM4', 38400, timeout= 0.1)
	#ser = serial.Serial('COM19', 9600, timeout=0.1)
	return ser	
def fGetCurrentPos(ser,PosX,PosY,PosZ):
	CurrentGpsMsg = ser.readline()
	# print 'CurrentGPSMsg = ',CurrentGpsMsg
	GpsMsgArray  = CurrentGpsMsg.split(',')
	CurrentGPSx  = PosX
	CurrentGPSy  = PosY
	CurrentGPSz  = PosZ
	GPSStat 	 = 'None'
	GPSSol		 = 'None'
	if len(GpsMsgArray) == 32:
		if GpsMsgArray[0] == '#BESTUTMA':
			CurrentGPSx = GpsMsgArray[13]
			CurrentGPSy = GpsMsgArray[14]
			CurrentGPSz = GpsMsgArray[15]
			CurrentTime = GpsMsgArray[6]
			GPSStat 	= GpsMsgArray[9]
			GPSSol      = GpsMsgArray[10]
	return (float(CurrentGPSx),float(CurrentGPSy),float(CurrentGPSz),GPSStat,GPSSol)
	
def fCheckCommand(vSTATUS):
	vCOMMAND = 0
	if msvcrt.kbhit():
		vInput = msvcrt.getch()
		if (vInput == 'w') :
			vCOMMAND = Forward
			vSTATUS = 'MANUAL'
			print '^.^'
		if (vInput == 'a') :
			vCOMMAND = Left
			vSTATUS = 'MANUAL'
			print '<<.'
		if (vInput == 'd') :
			vCOMMAND = Right
			vSTATUS = 'MANUAL'
			print '.>>'
		if (vInput == 'z') :
			vCOMMAND = RotL
			vSTATUS = 'MANUAL'
			print 'Rotate'			
		if (vInput == 's') :
			vCOMMAND = Stop
			vSTATUS = 'MANUAL'
			print 'STOP.'
		if (vInput == 'i') :
			vSTATUS = 'INIT'
		if (vInput == 'm') :
			vSTATUS = 'MANUAL'
		if (vInput == 'u') :
			vSTATUS = 'AUTO'
		if (vInput == 'c') :
			vSTATUS = 'CONFIG'
		if (vInput == '?') :
			vCOMMAND = 0
			print 'w','Forward'
			print 'a','Left'
			print 'd','Right'
			print 's','Stop'
			print 'i','INIT'
			print 'm','MANUAL'
			print 'u','AUTO'
			print 'c','CONFIG'
	return (vCOMMAND,vSTATUS)
	
def fSendCommand(BotPort,vCOMMAND,vDelayBwCmd,vDelayBwCmd2):
	if vCOMMAND != 0 :
		BotPort.write(vCOMMAND)
		# print 'fSendCommand >>', vCOMMAND
		if vCOMMAND != Stop:
			time.sleep(vDelayBwCmd)	
			vCOMMAND = Forward
			BotPort.write(vCOMMAND)
			time.sleep(vDelayBwCmd2)	
			vCOMMAND = Stop
			BotPort.write(vCOMMAND)
		
	
#******************************************************************************************
# PROGRAM START
#******************************************************************************************
vSTATUS  = 'INIT'
vCOMMAND = chr(0x11)
BotPort	= fInitBotPort()
GpsPort = fInitGpsPort()
PosX	= 0
PosY	= 0
PosZ 	= 0
vDelayBwCmd   = 0.8
vDelayBwCmd2   = 0.8
vDelayBwCyc   = 1
vForwardCount = 0
vMaxForward	  = 2
vWaitForward  = 10  # NUMBER OF CYCLES TO WAIT FOR INIT. 
EndRange	  = 5 # Distance from origin in meters
EndHead  	  = 45 # Angle from origin in degree
PosErr 		  = 0.5  # Error in posiiton error
AngleErr  	  = 1  # deg
TxReady 	  = 1  # Ready to Transmit next command
PrvX 		  = 0
PrvY 		  = 0

			
print 'Type ? for help'
Counter 	= 0
MaxCounter 	= 250
while( Counter < MaxCounter):
	CmdChar = 'None'
	TcycleStart = datetime.datetime.now()
	(vCOMMAND,vSTATUS) = fCheckCommand(vSTATUS)
	(PosX,PosY,PosZ,GPSStat,GPSSol)  = fGetCurrentPos(GpsPort,PosX,PosY,PosZ)
	# print 'Main >', GPSStat, GPSSol
#*********************** AUTO
	if  (vSTATUS == 'AUTO'):
		if TxReady == 1:

			if (abs(PosX - EndPosX) < PosErr ) & (abs(PosY - EndPosY) < PosErr ):
				print vSTATUS+'>'+'Mission Complete !. Returning to Manual Control'
				vSTATUS = 'MANUAL'
			else: 
				CurrHead = fReturnHeading(PrvX,PrvY,PosX,PosY) #xstart,ystart,xend,yend
				NewHead  = fReturnHeading(PosX,PosY,EndPosX,EndPosY) #xstart,ystart,xend,yend
				if (abs( NewHead - CurrHead)) > 180:
					ReqHeading = (360 + NewHead - CurrHead )%360
				else:
					ReqHeading = (360 + CurrHead - NewHead )%360

				print NewHead,'-', CurrHead,'=',ReqHeading 
				
				if (abs(ReqHeading) < AngleErr ) | (abs(ReqHeading - 360) < AngleErr ):
					vCOMMAND = Forward
					CmdChar =  'Fwrd'
				elif 0 <= ReqHeading < 180:
					vCOMMAND = Right
					CmdChar =  'Rght'
				elif 180 <= ReqHeading < 360:
					vCOMMAND = Left
					CmdChar =  'Left'				
				print vSTATUS+'>','Cmd =',CmdChar,'NewHead =',round(NewHead,3),'CurrHead =',round(CurrHead,3),'ReqHead =', round(ReqHeading,3), 'X,EndX :: Y,EndY =','||',round(PosX-OrginX,2),round(EndPosX - OrginX,2),'::' ,round(PosY - OrginY,2) ,round(EndPosY - OrginY,2),'||' ,'C =',Counter
			xlist = xlist + [PosX-OrginX ]
			ylist = ylist + [PosY-OrginY ]
			tlist = tlist + [Tcycleend.minute*60+ Tcycleend.second + Tcycleend.microsecond*0.000001]
			PrvX = PosX
			PrvY = PosY
			TxReady = 0
		elif TxReady == 0:
			if( (abs(PrvX - PosX) > 0.05) | (abs(PrvY - PosY) > 0.05) ):
				TxReady = 1
			print vSTATUS+'>','TxReady =',TxReady,'PrvX=',PrvX,'PosX=',PosX,'PrvY=',PrvY,'PosY=',PosY
#*********************** MANUAL

	elif(vSTATUS == 'MANUAL'):
		print vSTATUS+'>',PosX, PosY, PosZ,'>>'
		
#*********************** INIT		
	elif(vSTATUS == 'INIT'):
		if vForwardCount == 0:
			InitPosX = PosX
			InitPosY = PosY
			if ( ( PosX == 0) | ( PosY == 0)):
				vForwardCount = vForwardCount - 1
		if vForwardCount < vMaxForward:
			vForwardCount = vForwardCount + 1
			vCOMMAND 	  = Forward
		elif vForwardCount >= vMaxForward:
			vCOMMAND = Stop
			if ( vWaitForward <= 0):
				OrginX 	 = PosX
				OrginY   = PosY
				InitHead = fReturnHeading(InitPosX,InitPosY,OrginX,OrginY) #xstart,ystart,xend,yend
				print 'StartHeading =', InitHead
				EndPosX	 = OrginX + EndRange*math.cos((InitHead + EndHead)*math.pi/180)
				EndPosY  = OrginY + EndRange*math.sin((InitHead + EndHead)*math.pi/180)
				PrvX     = InitPosX
				PrvY 	 = InitPosY
				vSTATUS  = 'AUTO'
			vWaitForward = vWaitForward - 1
		print vSTATUS+'>'+'FwdCount =',vForwardCount
		
#*********************** CONFIG		
	elif(vSTATUS == 'CONFIG'):
		print vSTATUS+'>'
	fSendCommand(BotPort,vCOMMAND,vDelayBwCmd,vDelayBwCmd2)
	Tcycleend = datetime.datetime.now()

	Tdiff =  Tcycleend - TcycleStart
	time.sleep(vDelayBwCyc - Tdiff.microseconds*0.000001)	
	Counter 	= Counter + 1
plt.plot(xlist, ylist, 'ro')
plt.axis([min(xlist),max(xlist), min(ylist), max(ylist)]) 		
plt.show()

print "Exiting program !!! "
